//functions.c
//
//
#include "system.h"

extern int rxState; // 0-Preamble search,   1-Post-preamble

SignalState signalState = RED;
	
comPacket initComPacket(int isGroup,int isPing,int isStartup,int isWarning,int isResponse,
		int groupNumber,int packetNumber,int length,int destination, int source,unsigned char * data)
{
	comPacket thisPacket;
	int checksum = 0;
	int i = 0;
	
	//Set the bytes of the packet
	thisPacket.PRE = 0xA3;
	thisPacket.DES = (byte)(destination & 0xff);
	thisPacket.SRC = (byte)(source & 0xff);
	thisPacket.FLG = (byte)((isGroup ? (byte)0x01 : (byte)0) |
							(isPing ? (byte)0x80 : (byte)0) |
							(isStartup ? (byte)0x40 : (byte)0) |
							(isWarning ? (byte)0x20 : (byte)0) |
							(isResponse ? (byte)0x10 : (byte)0)
							);
	thisPacket.GRP = (byte)((((groupNumber & 0x0f) << 4) | (packetNumber & 0x0f)) & 0xff);
	thisPacket.LEN = (byte)(length & 0xff);
	memcpy(thisPacket.DAT, data, length);
	//compute the checksum
	checksum += thisPacket.PRE;
	checksum += thisPacket.DES;
	checksum += thisPacket.SRC;
	checksum += thisPacket.FLG;
	checksum += thisPacket.GRP;
	checksum += thisPacket.LEN;
	for(i=0; i<length; i++)
	{
		//sendChar((char)i);
		checksum += (byte)(data[i] & 0xff);
	}
	
	thisPacket.CHK = (byte) (checksum & 0xff);
	//sendChar(thisPacket.CHK);
	//sendChar((char)checksum);
	
	return thisPacket;
}//computerPacket

//Take a comPacket Struct and put it into a packet
int toByteArray(byte * bytes, comPacket * packet)
{
	
	bytes[0] = packet->PRE;
	bytes[1] = packet->DES;
	bytes[2] = packet->SRC;
	bytes[3] = packet->FLG;
	if(packet->FLG & 0x01)
	{
		bytes[4] = packet->GRP;
		bytes[5] = packet->LEN;
		//sendChar('D');
		//sendChar(bytes[6]);
		memcpy(&bytes[6], packet->DAT, packet->LEN);
		//sendChar('D');
		//sendChar(bytes[6]);
		bytes[6+packet->LEN] = packet->CHK;
		return packet->LEN + 7;
	}
	else
	{
		bytes[4] = packet->LEN;
		memcpy(&bytes[5], packet->DAT, packet->LEN);
		bytes[5+packet->LEN] = packet->CHK;
		return packet->LEN + 6;
	}
	
	
	return 0;
}//toByteArray


//store the data being recieved into a global comPacket Struct and alert when filled
//
void store_DataIn(byte inByte)
{
	static comPacket thisPacket;
	static int state =0;	//Static variable to remember what byte of the packet we should be recieving next
							//0-Pre, 1-DES, 2-SRC, 3-FLG, 4-GRP, 5-LEN, 6-DAT, 7-CHK
	static int bytesLeft =0;
	static int bytesRead =0;
	static byte checkSum =0;
	//LED3 = 0;

		//sendChar('S');
		//sendChar((char)(state));
		//sendChar((char)inByte);
	//LED1 = 0;
	switch (state)
	{
		case 0:	//PRE state
			if(inByte != 0xA3)
			{
				break;
			}
		//	LED4 = 0;
			thisPacket.PRE = inByte;  	//set the preamble byte of the packet
			checkSum += inByte;			//Update our checkSum	
			state = 1;	//update the state to Destination
			break;
		case 1:	//DES state
			if ((inByte != ADDRESS) && (inByte != 0xFF))
			{
				state = 0;
				rxState = 0;
				checkSum = 0;
				break;
			}
			thisPacket.DES = inByte;	//set the destination byte of the packet
			checkSum += inByte;			//Update our checksum
			state = 2;	//update the state to Source;
			break;
		case 2:	//SRC state
			thisPacket.SRC = inByte;	//set the source byte of the packet
			checkSum += inByte;			//Update our checksum
			state =3;
			break;
		case 3:	//FLG state
			thisPacket.FLG = inByte;	//Set the FLag byte of the packet
			checkSum += inByte;			//Update our checksum
			if((inByte & 0x01) > 0)
			{
				state = 4;	//update the state to Group
			}
			else
			{
				thisPacket.GRP =0; //No Group so set Group Byte to zero
				state = 5;	//Update the state to Length
			}
			break;
		case 4:	//GRP state
			thisPacket.GRP = inByte;	//Set the group byte of the packet
			checkSum += inByte;			//Update our checksum
			state =5;					//Update state to Length
			break;
		case 5:	//LEN State
			thisPacket.LEN = inByte; 	//Set the length byte of the packet
			checkSum += inByte;			//Update our checksum
			bytesLeft = inByte;			//set the number of bytes of data that we need to read
			bytesRead = 0;				//Set the number of bytes that we have read
			if (inByte == 0) state = 7;
			else state = 6;					//Update the state to Data
			break;
		case 6:	//DAT state
			bytesLeft--;			//Decrement the number of bytes that we have left to read;
			thisPacket.DAT[bytesRead] = inByte;	//put the byte into the data field of the packet
			checkSum += inByte;			//Update our checksum
			bytesRead++; 			//INcrement the number of bytes that we have read;

			if(bytesLeft == 0)
			{
				state =7;			//Set state to CHK
			}
			break;
		case 7:	//CHK State
			thisPacket.CHK = inByte;
			//sendChar(checkSum);
			if(inByte != checkSum)
			{
				state =0;
				checkSum = 0;
				break;
			}
			doPacket(&thisPacket);	//Do what we want to do with the packet
			checkSum =0;			//reset our checksum
			state =0;				//set the state to PRE
			break;
	}//switch
}

void doPacket(comPacket * packet)
{
	int i, j;

	if(packet->FLG & 0x80)			//Ping Packet handle
	{
		if(packet->FLG & 0x10)		
		{
			//printf("Ping response received from: %d\n", packet->SRC);
			//is a response
			//sendPing(1);
			return;
		}
		else
		{
			//printf("Ping request received from: %d\n", packet->SRC);
			//printf("Sending ping response ...\n");
			sendPing(1);// send response ping
			return;
		}
	}
	else if (packet->FLG & 0x40)	//Startup packet
	{
		if (packet->FLG & 0x20)
		{
			//printf("Warning at startup from: %d\n", packet->SRC);
			//printf("Message: ");
			//for(i=0;i<packet->LEN;i++) putchar(packet->DAT[i]);
			//printf("\n");
			// is warning at startup
			return;
		}
		else
		{
			LED4 = 0;
			for (i=0; i<ADDRESS*500; i++)
			{
				for (j=0; j<50; j++);
			}
			asm ("RESET");
			return;
		}
	}
	else if (packet->FLG & 0x20)
	{
		// is warning packet
		//printf("Warning message from: %d\n", packet->SRC);
		//printf("Message: ");
		//for(i=0;i<packet->LEN;i++) putchar(packet->DAT[i]);
		//printf("\n");
		return;
	}
	else if (packet->FLG & 0x10)
	{
		// is response packet
		//printf("Response from: %d\n", packet->SRC);
		//printf("Message: ");
		//for(i=0;i<packet->LEN;i++) putchar(packet->DAT[i]);
		//printf("\n");
		return;
	}
	else
	{
		doApplication(packet->DAT);
	}
	return;
}

void doApplication(byte data[])
{
	switch (data[0])
	{
		case 0x80:
			if (data[1] == ADDRESS) signalState = data[2];
			break;
		default:
			break;
	}
}

void sendPing(int isResponse)
{
	int i;
	comPacket packet;
	byte bytes[MaxPacketLength];
	unsigned char dataRes[13] = "Ping Response";
	unsigned char dataReq[13] = "Ping Request!";

	if (isResponse)
	{
		packet = initComPacket(0, 1, 0, 0, 1, 0, 0, 13, 0, ADDRESS, dataRes);
	}
	else
	{
		packet = initComPacket(0, 1, 0, 0, 0, 0, 0, 13, 0, ADDRESS, dataReq);
	}

	//sendCharArray(&packet, (packet.LEN+6));

	toByteArray(bytes, &packet);

	for (i=0; i<(packet.LEN+6); i++)
	{
		sendChar(bytes[i]);
	}

	return;
}

void sendStartup(int isWarning, char * message, int mlen)
{
	int i;
	comPacket packet;
	byte bytes[MaxPacketLength];

	

	if (isWarning)
	{
		packet = initComPacket(0, 0, 1, 1, 0, 0, 0, mlen, 0, ADDRESS, (byte *)message);
	}
	else
	{
		packet = initComPacket(0, 0, 1, 0, 0, 0, 0, mlen, 0, ADDRESS, (byte *)message);
	}

	i = toByteArray(bytes, &packet);

	sendCharArray((char *)bytes, i);

	return;
}

void sendCMDFront(byte* trainId,byte* results)
{
	int i;
	comPacket packet;
	byte bytes[MaxPacketLength];
	byte data[15];

	data[0] = 0x00;
	data[1] = ADDRESS;
	data[2] = trainId[1]; // Train ID bits 63:56
	data[3] = trainId[2]; // Train ID bits 55:48
	data[4] = trainId[3]; // Train ID bits 47:40
	data[5] = trainId[4]; // Train ID bits 39:32
	data[6] = trainId[5]; // Train ID bits 31:24
	data[7] = trainId[6]; // Train ID bits 23:16
	data[8] = trainId[7]; // Train ID bits 15:8
	data[9] = trainId[8]; // Train ID bits 7:0
	data[10] = results[0]; // Timer bits 31:24
	data[11] = results[1]; // Timer bits 23:16
	data[12] = results[2]; // Timer bits 15:8
	data[13] = results[3]; // Timer bits 7:0
	data[14] = results[4]; // Direction

	packet = initComPacket(0, 0, 0, 0, 0, 0, 0, 15, 0, ADDRESS, data);

	i = toByteArray(bytes, &packet);

	sendCharArray((char *)bytes, i);

	return;
}

void sendCMDBack(byte* trainId, byte *results)
{
	int i;
	comPacket packet;
	byte bytes[MaxPacketLength];
	byte data[16];

	data[0] = 0x01;		//Op Code
	data[1] = ADDRESS;	//Sensor ID
	data[2] = trainId[1]; // Train ID bits 63:56
	data[3] = trainId[2]; // Train ID bits 55:48
	data[4] = trainId[3]; // Train ID bits 47:40
	data[5] = trainId[4]; // Train ID bits 39:32
	data[6] = trainId[5]; // Train ID bits 31:24
	data[7] = trainId[6]; // Train ID bits 23:16
	data[8] = trainId[7]; // Train ID bits 15:8
	data[9] = trainId[8]; // Train ID bits 7:0
	data[10] = results[0]; // Timer bits 31:24
	data[11] = results[1]; // Timer bits 23:16
	data[12] = results[2]; // Timer bits 15:8
	data[13] = results[3]; // Timer bits 7:0
	data[14] = results[4]; // Direction
	data[15] = results[5]; //number of cars

	packet = initComPacket(0, 0, 0, 0, 0, 0, 0, 16, 0, ADDRESS, data);

	i=toByteArray(bytes, &packet);

	sendCharArray((char *)bytes,i);

	return;
}

void sendCMDError(byte errorCode)
{
	int i;
	comPacket packet;
	byte bytes[MaxPacketLength];
	byte data[2];
	
	data[0] = 0x02;
	data[1] = ADDRESS;
	data[2] = errorCode;

	packet = initComPacket(0, 0, 0, 0, 0, 0, 0, 3, 0, ADDRESS, data);

	i = toByteArray(bytes, &packet);

	sendCharArray((char *)bytes, i);
	return;
}

